/*
 * Name       : prosper_user_node.h
 * Author     : Xiaoafei
 * Copyright  : 2017 The Prosper Tech
 */

#ifndef __PROSPER_USER_NODE_H
#define __PROSPER_USER_NODE_H

#include <string>

#include "ros/ros.h"
#include "tf/tf.h"

#include <geometry_msgs/Twist.h>

#include <nav_msgs/Odometry.h>

#include "prosper_underpan/robot_pose_info.h"
#include "prosper_underpan/emergent_info.h"
#include "prosper_underpan/main_task_info.h"
#include "prosper_underpan/sub_task_info.h"
#include "prosper_underpan/robot_status_info.h"
#include "prosper_underpan/task_status_info.h"
#include "prosper_underpan/pose_ctl_task_info.h"

#include <fstream>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/PoseArray.h>
#include <tf/transform_listener.h>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>

#include "robot_driver_base.h"
#include <string>
#include <stdio.h>


using namespace std;
using namespace AgvAssistant;

/**
* @brief prosper机器人平台通讯接口相关类
*/
namespace prosper_user{

/**
* @brief prosper用户接口
*/
class ProsperUser : public RobotDriverBase{

public:
    /**
     * @brief 创建prosper机器人平台用户接口驱动对象
     * @param name [in] 驱动名称，用于参数查找的图元名称前缀
     */
    ProsperUser(std::string name);
    ~ProsperUser();

private:
    ros::NodeHandle nh_;           ///< 全局句柄
    ros::NodeHandle nh_private_;   ///< 私有句柄,用驱动名称作为图元名称前缀

    ros::Publisher vel_pub_;       ///< 机器人速度控制命令发布器
    ros::Subscriber odom_sub_;     ///< 用于订阅轮式里程计信息
    ros::Subscriber world_pose_sub_;   ///< 用于订阅机器人全局位姿

    ros::Subscriber emergent_info_sub_;       ///< 订阅机器人急停事件

    ros::Subscriber robot_status_sub_;        ///< 机器人状态订阅

    ros::Subscriber self_def_task_info_sub_;   ///< 自定义任务订阅
    ros::Subscriber pose_ctl_task_status_info_sub_;  ///< 位姿控制任务状态信息订阅


    ros::Publisher pose_ctl_task_pub_;         ///< 机器人位姿调整请求发布器
    ros::Publisher task_status_pub_;           ///< 任务状态发布
    ros::Publisher robot_main_task_pub_;       ///< 机器人主任务发布

    boost::system::error_code openPort();

    /**
   * @brief 发布速度控制命令,该函数由子类根据不同机器人实现
   * @param x [in] 机器人X方向的速度(单位:m/s)，X轴正向一般为机器人正前方
   * @param y [in] 机器人Y方向的速度(单位:m/s)，Y轴正方向一般为垂直于机器人X方向向左
   * @param vth [in] 机器人旋转角速度(单位:rad/s),机器人逆时针旋转为正
   * @return 成功发布该命令返回true
   */
    bool pubVel(float x,float y,float vth);
    /**
     * @brief 发送机器人位姿调整请求，该函数有子类根据不同机器人实现
     *
     *  该函数发送某一位置相对当前位置的相对偏移来调整机器人位姿，当调整状态将通过'poseAck'消息反馈
     * @param request_code [in]
     *  1-基于里程计，借助底盘原始数据达到位置调节的功能，遥控和巡航模式通用
     *  2-基于SLAM，借助修正后的世界坐标调节机器人位置，一般用于巡航模式
     * @param switch_x [in] 机器人x，即前后方向是否做调整 是-true 否-false,该选项不一定对所有机器人都有效
     * @param switch_y [in] 机器人y，即左右方向是否做调整 是-true 否-false,该选项不一定对所有机器人都有效
     * @param switch_th [in] 机器人z，即角度是否做调整 是-true 否-false,该选项不一定对所有机器人都有效
     * @param x_set [in] x轴相对移动量(单位：m) +相对当前位置前进 -相对当前位置后退
     * @param y_set [in] y轴相对移动量(单位：m) +相对当前位置左平移 -相对当前位置右平移
     * @param th_set [in] z轴相对旋转量(单位：rad) +相对当前位置逆时针旋转 -相对当前位置顺时针旋转
     * @param max_vel_x [in] x轴调整最大线速度(单位：m/s),正值,该选项不一定对所有机器人都有效
     * @param max_vel_y [in] y轴调整最大线速度(单位：m/s),正值,该选项不一定对所有机器人都有效
     * @param max_vel_th [in] z轴调整最大角速度(单位：rad/s),正值,该选项不一定对所有机器人都有效
     * @param precision_x [in] x轴调节精度(单位：m),正值,该选项不一定对所有机器人都有效
     * @param precision_y [in] y轴调节精度(单位：m),正值,该选项不一定对所有机器人都有效
     * @param precision_th [in] z轴调节精度(单位：rad),正值,该选项不一定对所有机器人都有效
     * @return 成功发布请求放回true
     */
    bool sendPoseAdjustRequest(int request_code,
                                   bool switch_x,bool switch_y,bool switch_th,
                                   double x_set,double y_set,double th_set,
                                   double max_vel_x,double max_vel_y,double max_vel_th,
                                   double precision_x,double precision_y,double precision_th);
    /**
     * @brief 发送群控响应信息
     *
     *  当执行自定义群控任务后必须通过该函数返回对应任务的事件标识字符串
     * @param ack [in] 群控信息
     * @return 成功发送信息返回true
     */
    bool sendGroupControlAck(const GroupControlInfo& ack);

    unsigned char ascll_to_hex(char c)
    {
      int a = c;
      unsigned char hex = a & 0xff;
      return hex;
    }
    void string_to_hex(string s, std::vector<unsigned char>& hex)
    {
      for(char i = 0;i < s.size();i ++)
      {
        hex.push_back((ascll_to_hex(s[i])));
      }
    }

    unsigned char HexToChar(unsigned char bChar){
      if((bChar>=0x30)&&(bChar<=0x39)){
        //std::cout<<"1111111111111"<<std::endl;
        bChar -= 0x30;
      }

      else if((bChar>=0x41)&&(bChar<=0x46))//大写字母
      bChar -= 0x37;
      else if((bChar>=0x61)&&(bChar<=0x66))//小写字母
      bChar -= 0x57;
      else bChar = 0xAA;
      return bChar;
    }

    int comOpen(void);

    bool armSend(char order[],int size,int type);
    bool armSendReset(char order[],int size);
    bool serialCommunication(OrderClass order);

    /**
     * @brief 读取串口线程
     */
    void arm_working_exec();
    void odomCallBack(const nav_msgs::Odometry::ConstPtr &msg);
    void worldPoseUpdateCallBack(const prosper_underpan::robot_pose_info::ConstPtr& msg);
    void emergentInfoCallBack(const prosper_underpan::emergent_info::ConstPtr& msg);
    void robotStatusInfoCallBack(const prosper_underpan::robot_status_info::ConstPtr& msg);
    void selfDefTaskInfoCallback(const prosper_underpan::sub_task_info::ConstPtr& msg);
    void poseCtlTaskStatusInfoCallback(const prosper_underpan::task_status_info::ConstPtr& msg);



    void WriteTrailerStatusToYaml(bool is_trailer,double trailer_width, double trailer_length);


    void laserposeCallBack(const sensor_msgs::LaserScanConstPtr& laser);

    inline double absd(double data){
        if(data < 0)
            return -data;
        else
            return data;
    }


    inline double angle_normalize(double angle){
        while(angle>M_PI)
            angle -= 2*M_PI;

        while(angle <-M_PI)
            angle += 2*M_PI;

        return angle;
    }

    std::string driver_name_;   ///< 驱动名称，作为图元名称前缀
    tf::TransformListener listener_;
    ros::Subscriber laser_sub_;
    ros::Time stamp_;
    std::basic_string<char> frame_id_;
    std::vector<unsigned char> data_;
    int bytes_;
    bool write_flag_ = false;
    int type_;
    int trailer_;
};

}

 #endif
